#!/usr/bin/env python
# encoding: utf-8
# import public lib
from geometry_msgs.msg import Twist
import sys, select, termios, tty

# import ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32MultiArray as Float32MultiArray
from std_msgs.msg import Int16MultiArray

msg = """
Control Your SLAM-Bot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
t/T : x and y speed switch
s/S : stop keyboard control
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""

wheels_ctrl_Bindings = {
    "W": (-1, 1,-1,1),
    "A": (1, 1,-1,-1),
    "S": (1, -1,1,-1),
    "D": (-1, -1,1,1),
    "w": (-1, 1,-1,1),
    "a": (1, 1,-1,-1),
    "s": (1, -1,1,-1),
    "d": (-1, -1,1,1),
    "t": (0, 0,0,0),
    "T": (0, 0,0,0),
    "Q":(1,1, 1,1),
    "q":(1, 1,1,1),
    "E":(-1,-1, -1,-1),
    "e":(-1,-1, -1,-1),
}

moveBindings = {
    "i": (1, 0),
    "o": (1, -1),
    "j": (0, 1),
    "l": (0, -1),
    "u": (1, 1),
    ",": (-1, 0),
    ".": (-1, 1),
    "m": (-1, -1),
    "I": (1, 0),
    "O": (1, -1),
    "J": (0, 1),
    "L": (0, -1),
    "U": (1, 1),
    "M": (-1, -1),
}

speedBindings = {
    "Q": (1.1, 1.1),
    "Z": (0.9, 0.9),
    "W": (1.1, 1),
    "X": (0.9, 1),
    "E": (1, 1.1),
    "C": (1, 0.9),
    "q": (1.1, 1.1),
    "z": (0.9, 0.9),
    "w": (1.1, 1),
    "x": (0.9, 1),
    "e": (1, 1.1),
    "c": (1, 0.9),
}


class Yahboom_Keybord(Node):
    """用于控制SLAM-Bot的键盘输入类"""

    def __init__(self, name):
        """初始化类的实例"""
        super().__init__(name)
        # self.pub = self.create_publisher(Float32MultiArray, "wheels_speed_set", 10)
        self.intpub=self.create_publisher(Int16MultiArray, "wheels_speed_set", 10)
        # 初始化键盘控制
        
        self.settings = termios.tcgetattr(sys.stdin)
        

    def getKey(self):
        """获取键盘输入的一个按键"""
        # 设置输入模式为非阻塞
        tty.setraw(sys.stdin.fileno())
        # 获取输入
        rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
        # 处理输入
        if rlist:
            key = sys.stdin.read(1)
        else:
            key = ""
        # 设置输入模式为正常
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
        return key

    def vels(speed, turn):
        """返回当前速度和转向信息"""
        return "currently:\tspeed %s\tturn %s " % (speed, turn)


def main():
    """程序主入口"""
    rclpy.init()
    yahboom_keyboard = Yahboom_Keybord("yahboom_keyboard_ctrl")
    stop = False
    wheelspd=[0,0,0,0]
    try:
        while 1:
            key = yahboom_keyboard.getKey()
            if key == "X" or key == "x":
                exit()
            # 如果key是moveBindings中的键，则更新速度
            if key in wheels_ctrl_Bindings.keys():
                wheelspd = list(wheels_ctrl_Bindings[key])
                for i in range(4):
                    wheelspd[i]=int(wheelspd[i]*1000+1500)
                # publish msg
                # msg = Float32MultiArray(data=wheelspd)
                int_msg=Int16MultiArray(data=wheelspd)
                # yahboom_keyboard.pub.publish(msg)
                yahboom_keyboard.intpub.publish(int_msg)
                print("wheels speed set: ", int_msg.data)
                

    except Exception as e:
        print(e)

if __name__ == "__main__":
    main()
